package strauss.flatland.things.geometry;

import strauss.flatland.things.Dot;
import strauss.flatland.things.IThing;
import strauss.flatland.things.Line;

public class Collision {
	public static void doCollisionResponse(IThing a, IThing b, Vertex collisionPoint) {
		doCollision2(a,b,collisionPoint);
	}
	
	public static void doCollision1(IThing a, IThing b, Vertex collisionPoint) {
//		 lets do perfectly elastic collisions for now
		GeometricVector aInput = a.getGeometricVector();
		GeometricVector bInput = b.getGeometricVector();
		final double ELASTICITY = 1;
		
		// right now the calculation of out normal is very pokey, we 
		// only take into account points colliding with each other!
		//normal = new GeometricVector(0.5, 0.5);
		
		// based on "Physics for Game Developers"
		// calculate the line of action
		GeometricVector aNormalized = VectorMath.normalize(aInput);
		GeometricVector bNormalized = VectorMath.normalize(bInput);
		GeometricVector lineOfAction = VectorMath.add(aNormalized, bNormalized);
		if (VectorMath.isZero(lineOfAction)) {
			lineOfAction = aNormalized; // then we can choose any one of them
		}
		
		// v = relative velocity along line of action of impact
		// v = v1-v2
		GeometricVector relativeVelocity = VectorMath.delta(aInput, bInput);
		
		// Linear impulse
		// J = -v(e+1)/(1/m1 + 1/m2)
		GeometricVector tmpVector1 = VectorMath.multiply(relativeVelocity, -1.0*(ELASTICITY+1.0));
		double tmpDouble = 1.0/a.getMass()+1.0/b.getMass();
		GeometricVector linearImpulse = VectorMath.divide(tmpVector1,tmpDouble);
		
		// resultant velocity = Linear impulse, divided by mass plus original vector
		// v1 = J/m1 + v1
		// v2 = J/m2 + v2
		//aInput.add(VectorMath.divide(linearImpulse, a.getMass()));
		//bInput.add(VectorMath.divide(VectorMath.multiply(linearImpulse,-1), b.getMass()));
		
		// resultant linear velocity
		// v1 = v1 + (Jn)/m1
		// v2 = v2 + (Jn)/m2
		tmpDouble = VectorMath.dotProduct(linearImpulse, lineOfAction);
		aInput.add(tmpDouble / a.getMass());
		tmpDouble = VectorMath.dotProduct(VectorMath.multiply(linearImpulse,-1), lineOfAction);
		bInput.add(tmpDouble / b.getMass());
	}
	
	public static void doCollision2(IThing a, IThing b, Vertex collisionPoint) {
		final double ELASTICITY = 1;
		GeometricVector relativeVelocity = VectorMath.delta(a.getGeometricVector(), b.getGeometricVector());
		
		GeometricVector lineOfAction = null;
		if (a instanceof Dot && b instanceof Dot) {
			lineOfAction = getDotDotActionLine(a, b);
		} else if (a instanceof Line ^ b instanceof Line) {
			if (a instanceof Line) {
				lineOfAction = getDotLineActionLine((Dot)b, (Line)a);
			} else {
				lineOfAction = getDotLineActionLine((Dot)a, (Line)b);
			}
		}
		
		double part1 = -(1.0+ELASTICITY);
		double part2 = VectorMath.dotProduct(relativeVelocity, lineOfAction);
		double part3 = VectorMath.dotProduct(lineOfAction, lineOfAction);
		double part4 = 1/a.getMass() + 1/b.getMass();
		
		double j = (part1 * part2) / (part3 * part4);
		
		lineOfAction.multiply(j);
		a.getGeometricVector().add( VectorMath.divide(lineOfAction, a.getMass())  );
		GeometricVector tmp = VectorMath.divide(lineOfAction, b.getMass());
		b.getGeometricVector().subtract( tmp );
	}

	private static GeometricVector getDotLineActionLine(Dot dot, Line line) {
		/*GeometricVector tmpa = new GeometricVector(line.getEnd().getX()-line.getStart().getX(), line.getEnd().getY()-line.getStart().getY());
		tmpa.normalize();
		tmpa.rotate(Math.PI/2);
		GeometricVector tmpb = VectorMath.normalize(dot.getGeometricVector());
		GeometricVector lineOfAction = VectorMath.add(tmpa, tmpb);
		if (VectorMath.isZero(lineOfAction)) {
			lineOfAction = tmpa;
		} else {
			lineOfAction.normalize();
		}
		*/
		GeometricVector tmpa = new GeometricVector(line.getEnd().getX()-line.getStart().getX(), line.getEnd().getY()-line.getStart().getY());
		tmpa.normalize();
		tmpa.rotate(Math.PI/2);
		return tmpa;
	}

	private static GeometricVector getDotDotActionLine(IThing a, IThing b) {
		GeometricVector aNormalized = VectorMath.normalize(a.getGeometricVector());
		GeometricVector bNormalized = VectorMath.normalize(b.getGeometricVector());
		GeometricVector lineOfAction = VectorMath.add(aNormalized, bNormalized);
		if (VectorMath.isZero(lineOfAction)) {
			lineOfAction = aNormalized; // then we can choose any one of them
		} else {
			lineOfAction.normalize();
		}
		return lineOfAction;
	}
}
